#ifndef SPLINE_HPP
#define SPLINE_HPP
#include <iostream>
#include <exception>
namespace ohe_framework
{
class CircularBuffer
{
private :
    float* values;
    int write_index;
    int read_index;
    int no_of_slots;
    void clear()
    {
        read_index = -1;
        write_index = 0;
        values = new float[no_of_slots];
        for(int i =0; i<no_of_slots;i++)
        {
            values[i] =0 ;
        }
    }

public:
    CircularBuffer(int slot=0)
    {
        no_of_slots = slot;
        clear();
    }
    ~CircularBuffer()
    {
        if(values!=0)
            delete values;
    }
    void write(const float &value)
    {
        values[write_index] = value;
        write_index = (write_index+1)%no_of_slots;
    }
    float read(int index)
    {
        index = (index+no_of_slots)%no_of_slots;
        return values[index];
    }
    int getWriteIndex()
    {
        return ((write_index+no_of_slots-1)%no_of_slots);
    }
};
    
class Spline
{
  private:
    CircularBuffer *grad_height;
    CircularBuffer *grad_stagger;
    CircularBuffer *grad2_height;
    CircularBuffer *grad2_stagger;
    CircularBuffer *inputs;
    CircularBuffer *diff_input;
    CircularBuffer *heights;
    CircularBuffer *staggers;
    CircularBuffer *points;
    float** m1;//[100][100];
    float** m2;//[100][100];
    float* s1;
    float* s2;
    int size;
    void reCalculateMatrixes();
    void forwardCalculation();
    void backwordCalculation();
    int count;
    float acce_stagger,acce_height;
  public:
    Spline(int point);
    ~Spline();
    void addPoint(float x, float height,float stagger,int point);
    void getPoints(float *&height_points, float *&stagger_points, int ref);
    void clear();
};

}// end namespace ohe_framework
#endif
